/**
 * Copyright (C) 2025 ROKAE (Beijing) Technology Co., LTD.
 * All Rights Reserved.
 *
 * Information in this file is the intellectual property of Rokae Technology Co., Ltd,
 * And may contains trade secrets that must be stored and viewed confidentially.
 *
 * @file InspireRobotsAPI.cpp
 * @brief 插件操作接口
 */
#include "InspireRobotsAPI.h"

// 全局对象用于操作夹爪（可以考虑替换为单例或静态函数）
InspireRobotsUtil::InspireRobotsAPI inspire_robots_api;

/**
 * @brief 启动夹爪设备（写入角度设置）
 * @param slave_id 从设备ID
 * @param angle_set0 角度设置0
 * @param angle_set1 角度设置1
 * @param angle_set2 角度设置2
 * @param angle_set3 角度设置3
 * @param angle_set4 角度设置4
 * @param angle_set5 角度设置5
 * @return true 成功
 * @return false 失败
 */
bool InspireRobotsUtil::InspireRobotsAPI::startInspireRobots(int slave_id,
                                                             int angle_set0, int angle_set1, int angle_set2,
                                                             int angle_set3, int angle_set4, int angle_set5)
{
    bool ret = false;
    const int maxRetries = 3;
    const int retryDelayUs = 10000;     // 每次重试间隔 10ms
    const int registerAddress = 0x05CE; // 起始寄存器地址
    const int dataLength = 6;           // 6 个寄存器

    for (int attempt = 1; attempt <= maxRetries; ++attempt)
    {
        ret = modbus_api.WriteRegister_10(
            slave_id,
            registerAddress,
            dataLength,
            {static_cast<uint16_t>(angle_set0),
             static_cast<uint16_t>(angle_set1),
             static_cast<uint16_t>(angle_set2),
             static_cast<uint16_t>(angle_set3),
             static_cast<uint16_t>(angle_set4),
             static_cast<uint16_t>(angle_set5)});

        if (ret)
            break;
        else
        {
            LOG(WARNING) << "WriteRegister_10 failed. Retry " << attempt << "/" << maxRetries;
            if (attempt < maxRetries)
                usleep(retryDelayUs);
        }
    }

    return ret;
}

/**
 * @brief 获取夹爪当前角度设置
 * @param slave_id 从设备ID
 * @param angle_set0 角度设置0
 * @param angle_set1 角度设置1
 * @param angle_set2 角度设置2
 * @param angle_set3 角度设置3
 * @param angle_set4 角度设置4
 * @param angle_set5 角度设置5
 * @return true 成功
 * @return false 失败
 */
bool InspireRobotsUtil::InspireRobotsAPI::setIDInspireRobots(int slave_id, int new_slave_id)
{
    bool ret = false;
    const int maxRetries = 3;
    const int retryDelayUs = 10000;     // 每次重试间隔 10ms
    const int registerAddress = 0x03E8; // 起始寄存器地址
    const int dataLength = 1;           // 1 个寄存器

    for (int attempt = 1; attempt <= maxRetries; ++attempt)
    {
        ret = modbus_api.WriteRegister_06(
            slave_id,
            registerAddress,
            dataLength,
            {static_cast<uint16_t>(new_slave_id)});

        if (ret)
            break;
        else
        {
            LOG(WARNING) << "WriteRegister_06 failed. Retry " << attempt << "/" << maxRetries;
            if (attempt < maxRetries)
                usleep(retryDelayUs);
        }
    }

    return ret;
}

/**
 * @brief 获取夹爪当前角度设置
 * @param slave_id 从设备ID
 * @param angle_set0 角度设置0
 * @param angle_set1 角度设置1
 * @param angle_set2 角度设置2
 * @param angle_set3 角度设置3
 * @param angle_set4 角度设置4
 * @param angle_set5 角度设置5
 * @return true 成功
 * @return false 失败
 */
bool InspireRobotsUtil::InspireRobotsAPI::getAngleInspireRobots(int slave_id,
                                                                int &angle_set0, int &angle_set1,
                                                                int &angle_set2, int &angle_set3,
                                                                int &angle_set4, int &angle_set5)
{
    bool ret = false;
    const int maxRetries = 3;
    const int retryDelayUs = 10000;     // 每次重试间隔 10ms
    const int registerAddress = 0x060A; // 起始寄存器地址
    const int dataLength = 6;           // 6 个寄存器

    for (int attempt = 1; attempt <= maxRetries; ++attempt)
    {
        std::vector<uint16_t> data;
        ret = modbus_api.ReadRegister_03(slave_id, registerAddress, dataLength, data);

        if (ret && data.size() == dataLength)
        {
            angle_set0 = data[0];
            angle_set1 = data[1];
            angle_set2 = data[2];
            angle_set3 = data[3];
            angle_set4 = data[4];
            angle_set5 = data[5];
            break;
        }
        else
        {
            LOG(WARNING) << "ReadRegister_03 failed or invalid data. Retry " << attempt << "/" << maxRetries;
            if (attempt < maxRetries)
                usleep(retryDelayUs);
        }
    }

    return ret;
}

/**
 * @brief 设置夹爪当前力设置
 * @param slave_id 从设备ID
 * @param force_set0 力设置0
 * @param force_set1 力设置1
 * @param force_set2 力设置2
 * @param force_set3 力设置3
 * @param force_set4 力设置4
 * @param force_set5 力设置5
 * @return true 成功
 * @return false 失败
 */
bool InspireRobotsUtil::InspireRobotsAPI::setForceInspireRobots(int slave_id,
                                                                int force_set0, int force_set1,
                                                                int force_set2, int force_set3,
                                                                int force_set4, int force_set5)
{
    bool ret = false;
    const int maxRetries = 3;
    const int retryDelayUs = 10000;     // 每次重试间隔 10ms
    const int registerAddress = 0x05DA; // 起始寄存器地址
    const int dataLength = 6;           // 6 个寄存器

    for (int attempt = 1; attempt <= maxRetries; ++attempt)
    {
        ret = modbus_api.WriteRegister_10(
            slave_id,
            registerAddress,
            dataLength,
            {static_cast<uint16_t>(force_set0),
             static_cast<uint16_t>(force_set1),
             static_cast<uint16_t>(force_set2),
             static_cast<uint16_t>(force_set3),
             static_cast<uint16_t>(force_set4),
             static_cast<uint16_t>(force_set5)});

        if (ret)
            break;
        else
        {
            LOG(WARNING) << "WriteRegister_10 failed. Retry " << attempt << "/" << maxRetries;
            if (attempt < maxRetries)
                usleep(retryDelayUs);
        }
    }

    return ret;
}

/**
 * @brief 设置夹爪当前速度设置
 * @param slave_id 从设备ID
 * @param speed_set0 速度设置0
 * @param speed_set1 速度设置1
 * @param speed_set2 速度设置2
 * @param speed_set3 速度设置3
 * @param speed_set4 速度设置4
 * @param speed_set5 速度设置5
 * @return true 成功
 * @return false 失败
 */
bool InspireRobotsUtil::InspireRobotsAPI::setSpeedInspireRobots(int slave_id,
                                                                int speed_set0, int speed_set1,
                                                                int speed_set2, int speed_set3,
                                                                int speed_set4, int speed_set5)
{
    bool ret = false;
    const int maxRetries = 3;
    const int retryDelayUs = 10000;     // 每次重试间隔 10ms
    const int registerAddress = 0x05F2; // 起始寄存器地址
    const int dataLength = 6;           // 6 个寄存器

    for (int attempt = 1; attempt <= maxRetries; ++attempt)
    {
        ret = modbus_api.WriteRegister_10(
            slave_id,
            registerAddress,
            dataLength,
            {static_cast<uint16_t>(speed_set0),
             static_cast<uint16_t>(speed_set1),
             static_cast<uint16_t>(speed_set2),
             static_cast<uint16_t>(speed_set3),
             static_cast<uint16_t>(speed_set4),
             static_cast<uint16_t>(speed_set5)});

        if (ret)
            break;
        else
        {
            LOG(WARNING) << "WriteRegister_10 failed. Retry " << attempt << "/" << maxRetries;
            if (attempt < maxRetries)
                usleep(retryDelayUs);
        }
    }

    return ret;
}